#include "usr_inc.h"

/* 定义推射模式 */
TossMode_t tossMode = ZERO_MODE;

/* 定义ID为3的3508电机实例 */
Dji_t dji3;
Pid_t velPid3;
Pid_t posPid3;
int32_t baseCnt3;

/**
 * @brief 初始化所有推射相关的实例
 */
void ALL_TOSS_Init(void)
{
    TOSS_SetServoAngle(180.0);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);

    DjiInit_t djiInit = {0};
    PidInit_t pidInit = {0};

    djiInit.motorID = 3;
    djiInit.hcan = &hcan2;
    DJI_Init(&dji3, &djiInit);

    pidInit.kp = 80.0;
    pidInit.ki = 0.0;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 15000 / 3;
    pidInit.maxOutput = 15000;
    pidInit.minOutput = -15000;
    PID_Init(&velPid3, &pidInit);

    pidInit.kp = 1200.0;
    pidInit.ki = 0.0;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 480 / 3;
    pidInit.maxOutput = 480;
    pidInit.minOutput = -480;
    PID_Init(&posPid3, &pidInit);
}

void StartTossUpdate(void *argument)
{
    /* 灭灯:ZERO_MODE; 亮灯:DRAG_MODE */
    HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);

    for (;;)
    {
        TossCmdMsg_t cmd;
        float stroke;

        if (xQueueReceive(tossCmdQueueHandle, &cmd, 2) == pdTRUE)
        {
            if (cmd == TOSS_DRAG)
            {
                tossMode = DRAG_MODE;
                HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);
            }
            else if (cmd == TOSS_FREE)
            {
                TOSS_SetServoAngle(180.0 - 110.0);

                vTaskDelay(pdMS_TO_TICKS(1200));

                TOSS_SetServoAngle(180.0);

                tossMode = ZERO_MODE;
                HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);
                PID_SetTarget(&posPid3, 0.0);
            }
            else if (cmd == TOSS_ABANDON)
            {
                tossMode = ZERO_MODE;
                HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);
                PID_SetTarget(&posPid3, 0.0);
            }
            else if (cmd == TOSS_UP)
            {
                PID_SetTarget(&posPid3, posPid3.target - 0.25);
            }
            else if (cmd == TOSS_DOWN)
            {
                PID_SetTarget(&posPid3, posPid3.target + 0.25);
            }
        }

        if (xQueueReceive(tossStrokeQueueHandle, &stroke, 2) == pdTRUE)
        {
            if (tossMode == DRAG_MODE)
            {
                if (stroke < 0.0)
                {
                    stroke = 0.0;
                }
                else if (stroke > 0.52)
                {
                    stroke = 0.52;
                }

                /* 导程20mm, 丝杆减速比20:24 */
                float tar = stroke / 0.020 * (20.0 / 24.0);
                PID_SetTarget(&posPid3, tar);
            }
        }
    }
}

void StartDji3Exe(void *argument)
{
    baseCnt3 = DJI_GetCnt(dji3InfoBoxHandle);

    TickType_t preWakeCnt = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        PID_Calculate(&posPid3, DJI_GetM3508Pos(dji3InfoBoxHandle, baseCnt3));
        PID_SetTarget(&velPid3, PID_GetOutput(&posPid3));
        PID_Calculate(&velPid3, DJI_GetM3508Rpm(dji3InfoBoxHandle));
        DJI_WriteCmdBuf(&dji3, PID_GetOutput(&velPid3), djiMutexHandle);

        vTaskDelayUntil(&preWakeCnt, pdMS_TO_TICKS(period));
    }
}

void StartTossPrintf(void *argument)
{
    vTaskSuspend(NULL);

    for (const int16_t period = 30; ;)
    {
        // PidArgsMsg_t msg;
        // if (xQueueReceive(pidArgsQueueHandle, &msg, 5) == pdTRUE)
        // {
        //     PID_SetArgs(&posPid3, msg.kp, msg.ki, msg.kd);
        //     PID_SetTarget(&posPid3, msg.target);
        // }

        printf("%f, %f, %f\r\n", DJI_GetM3508Pos(dji3InfoBoxHandle, baseCnt3), posPid3.target,
               (DJI_GetM3508Pos(dji3InfoBoxHandle, baseCnt3) - posPid3.target) * 0.020 / (20.0 / 24.0));

        vTaskDelay(pdMS_TO_TICKS(period));
    }
}

/**
 * @brief 设置扳机舵机的角度
 */
void TOSS_SetServoAngle(float angle)
{
    if (angle < 0.0)
    {
        angle = 0.0;
    }
    else if (angle > 180.0)
    {
        angle = 180.0;
    }

    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, (uint32_t)(angle / 180.0 * 2000.0) + 500);
}
